{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PreintegrationCombinedParams\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegrationCombinedParams.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `PreintegrationCombinedParams` class holds parameters specifically required for the `CombinedImuFactor` and its associated `PreintegratedCombinedMeasurements` class. \n",
    "\n",
    "It inherits from `PreintegrationParams` (and thus also `PreintegratedRotationParams`) and adds parameters that model the *evolution* of the IMU bias over time, typically as a random walk process. This is essential for the `CombinedImuFactor`, which estimates biases at both the start ($b_i$) and end ($b_j$) of the preintegration interval."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Parameters\n",
    "\n",
    "In addition to all parameters from `PreintegrationParams`, this class adds:\n",
    "\n",
    "- **`biasAccCovariance`**: A 3x3 matrix representing the continuous-time covariance of the *random walk process* driving the accelerometer bias. Units: (m²/s⁵)/Hz ? (Check docs, represents variance growth rate).\n",
    "- **`biasOmegaCovariance`**: A 3x3 matrix representing the continuous-time covariance of the *random walk process* driving the gyroscope bias. Units: (rad²/s³)/Hz ? (Check docs, represents variance growth rate).\n",
    "- **`biasAccOmegaInit`**: A 6x6 matrix representing the covariance of the uncertainty in the *initial* bias estimate provided to the preintegration. This accounts for the fact that the bias used for integration (`biasHat_`) is itself uncertain."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Background: Bias Random Walk\n",
    "\n",
    "The `CombinedImuFactor` implicitly assumes that the bias evolves between time steps according to a random walk:\n",
    "$$ b_{k+1} = b_k + w_k, \\quad w_k \\sim \\mathcal{N}(0, Q_b \\Delta t) $$ \n",
    "where $b_k = [b_{a,k}; b_{g,k}]$ is the 6D bias vector at time $k$, $w_k$ is zero-mean Gaussian noise, and $Q_b$ is the block-diagonal continuous-time covariance matrix formed from `biasAccCovariance` and `biasOmegaCovariance`:\n",
    "$$ Q_b = \\begin{bmatrix} \\text{biasAccCovariance} & 0 \\\\ 0 & \\text{biasOmegaCovariance} \\end{bmatrix} $$ \n",
    "The factor uses this model (integrated over the interval $\\Delta t_{ij}$) to constrain the difference between $b_i$ and $b_j$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Constructors**: \n",
    "  - `PreintegrationCombinedParams(n_gravity)`: Main constructor.\n",
    "  - `MakeSharedD(g=9.81)` / `MakeSharedU(g=9.81)`: Convenience static methods for NED/ENU frames.\n",
    "- **Setters**: Methods like `setBiasAccCovariance`, `setBiasOmegaCovariance`, `setBiasAccOmegaInit`, plus all setters inherited from `PreintegrationParams`.\n",
    "- **Getters**: Corresponding getters for the combined parameters, plus all inherited getters.\n",
    "- **`print` / `equals`**: Standard methods."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Create parameters, often starting from the base `PreintegrationParams` settings, and then set the bias evolution parameters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Combined IMU Preintegration Parameters:\n",
      "\n",
      "gyroscopeCovariance:\n",
      "[\n",
      "0.0001      0      0\n",
      "     0 0.0001      0\n",
      "     0      0 0.0001\n",
      "]\n",
      "accelerometerCovariance:\n",
      "[\n",
      "0.01    0    0\n",
      "   0 0.01    0\n",
      "   0    0 0.01\n",
      "]\n",
      "integrationCovariance:\n",
      "[\n",
      "1e-08     0     0\n",
      "    0 1e-08     0\n",
      "    0     0 1e-08\n",
      "]\n",
      "n_gravity = (    0     0 -9.81)\n",
      "biasAccCovariance:\n",
      "[\n",
      "1e-06     0     0\n",
      "    0 1e-06     0\n",
      "    0     0 1e-06\n",
      "]\n",
      "biasOmegaCovariance:\n",
      "[\n",
      "1e-08     0     0\n",
      "    0 1e-08     0\n",
      "    0     0 1e-08\n",
      "]\n",
      "biasAccOmegaInt:\n",
      "[\n",
      "  0.01      0      0      0      0      0\n",
      "     0   0.01      0      0      0      0\n",
      "     0      0   0.01      0      0      0\n",
      "     0      0      0 0.0025      0      0\n",
      "     0      0      0      0 0.0025      0\n",
      "     0      0      0      0      0 0.0025\n",
      "]\n"
     ]
    }
   ],
   "source": [
    "from gtsam import PreintegrationCombinedParams\n",
    "import numpy as np\n",
    "\n",
    "# 1. Create parameters for an ENU navigation frame (Z-up)\n",
    "params = PreintegrationCombinedParams.MakeSharedU(9.81)\n",
    "\n",
    "# 2. Set standard noise parameters (accel, gyro, integration)\n",
    "accel_noise_sigma = 0.1\n",
    "gyro_noise_sigma = 0.01\n",
    "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
    "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
    "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
    "\n",
    "# 3. Set bias random walk noise parameters (example values)\n",
    "bias_acc_rw_sigma = 0.001 # m/s^2 / sqrt(s) -> Covariance = sigma^2\n",
    "bias_gyro_rw_sigma = 0.0001 # rad/s / sqrt(s) -> Covariance = sigma^2\n",
    "params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)\n",
    "params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)\n",
    "\n",
    "# 4. Set initial bias uncertainty (covariance of bias used for preintegration)\n",
    "# Example: Assume bias estimate used for preintegration has some uncertainty\n",
    "initial_bias_acc_sigma = 0.1\n",
    "initial_bias_gyro_sigma = 0.05\n",
    "initial_bias_cov = np.diag(np.concatenate([\n",
    "    np.full(3, initial_bias_acc_sigma**2),\n",
    "    np.full(3, initial_bias_gyro_sigma**2)\n",
    "]))\n",
    "params.setBiasAccOmegaInit(initial_bias_cov)\n",
    "\n",
    "print(\"Combined IMU Preintegration Parameters:\")\n",
    "params.print()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These combined parameters are then passed to `PreintegratedCombinedMeasurements`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [PreintegrationCombinedParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationCombinedParams.h)\n",
    "- [CombinedImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.cpp) (Contains the definition of this class)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
